# -*- coding: utf-8 -*-
# """
# Created on 2021.11.10
# @author: FXY
# """
import numpy as np
import matplotlib.pyplot as plt
import math


# 函数：仿真设置和初始化
def INITIATION():
    # 下落目标初始状态和参数（英制单位）
    WARHEAD = {}
    WARHEAD['Y0'] = 3.28e5  # 初始高度（真实值）约为 100 km
    WARHEAD['X0'] = 2.62e4  # 落点和雷达间距离（真实值）约为 8 km
    WARHEAD['V0'] = -6.0e3  # 初始速度（真实值）向上为正
    WARHEAD['beta0'] = 500.0  # 目标分裂前的弹道系数（真实值）
    WARHEAD['beta1'] = 250.0  # 目标分裂后的等价弹道系数（真实值）
    WARHEAD['T_split'] = 30.0  # 目标发生分裂的时刻（真实值）
    # 雷达参数和初始测量值
    RADAR = {}
    RADAR['Ts'] = 0.01  # 测量时间间隔（秒）
    RADAR['sigma_theta'] = 0.005  # 角度测量精度（rad）
    RADAR['sigma_r'] = 25.0  # 距离测量精度（英尺）
    RADAR['R'] = 12.0  # 雷达天线距地面的高度（英尺）
    [r0, theta0] = Converto_rNtheta(WARHEAD['X0'], WARHEAD['Y0'], RADAR['R'])
    RADAR['r0'] = r0
    RADAR['r0_measured'] = r0 + RADAR['sigma_r']  # 距离初始测量值
    RADAR['theta0'] = theta0
    RADAR['theta0_measured'] = theta0 - RADAR['sigma_theta']  # 角度初始测量值
    # 仿真设置
    pars = {}
    pars['g'] = 32.2  # 重力加速度（为已知恒定值）！！取绝对值，而不是以向上为正
    pars['t0'] = 0.0  # 雷达开机时刻（秒）
    pars['tf'] = 60.0  # 雷达关机时刻（秒）
    pars['h'] = 0.001  # 目标下落过程的数值仿真积分步长（秒）
    return WARHEAD, RADAR, pars


# 函数：直角坐标(x, y)转换为极坐标(r, theta)
def Converto_rNtheta(x, y, R):
    r = math.sqrt(x ** 2 + (y - R) ** 2)
    theta = math.atan(x / (y - R))
    return r, theta


# 函数：极坐标(r, theta)转换为直角坐标(x, y)
def Converto_xNy(r, theta):
    # 提取 R
    WARHEAD, RADAR, pars = INITIATION()
    R = RADAR['R']
    # 计算 x 和 y
    x = r * math.sin(theta)
    y = R + r * math.cos(theta)
    return x, y


# 函数：下落目标的平面运动一阶ODE模型
def F_DYNAMICS(X, t):  # 大作业文档中的函数 DX = F(X)，X 是 numpy 数组
    # 提取所需的状态值
    x2 = X[1]
    x3 = X[2]
    x4 = X[3]
    # 提取受阻下落的运动参数
    WARHEAD, RADAR, pars = INITIATION()
    g = pars['g']
    if t - pars['t0'] <= WARHEAD['T_split']:
        beta = WARHEAD['beta0']
    else:
        beta = WARHEAD['beta1']
    # 计算4维状态变量的导数值
    DX = np.zeros(4)
    DX[0] = x2
    DX[2] = x4
    DX[3] = (0.0034 * g * (x4 ** 2) * math.exp(-x3 / 22000.0)) / (2.0 * beta) - g
    return DX


# 函数：四阶龙格库塔法
def RK4(X, t, h):  # 输入参数 X 就是 X_k
    K1 = F_DYNAMICS(X, t)  # F_DYNAMICS 就是 大作业RK4部分中定义的函数向量
    K2 = F_DYNAMICS(X + h * K1 / 2.0, t + h / 2.0)
    K3 = F_DYNAMICS(X + h * K2 / 2.0, t + h / 2.0)
    K4 = F_DYNAMICS(X + h * K3, t + h)
    X_next = X + h * (K1 + 2.0 * K2 + 2.0 * K3 + K4) / 6.0
    return X_next  # 积分所得的第 k+1 步的状态值


# *** 雷达测量的主函数定义 ***
def RADAR_MEASURES():
    ### 初始化
    # 生成采样时刻序列
    WARHEAD, RADAR, pars = INITIATION()
    h = pars['h']
    t0 = pars['t0']
    tf = pars['tf']
    TT = np.arange(t0, tf + h, h)
    # 根据大作业RK4部分中的定义，通过 INITIATION 初始化函数获取状态向量的初值 X0
    x1 = WARHEAD['X0']  # 落点和雷达间距离（真实值）
    x2 = 0.0  # 落点和雷达间距离的变化速度（真实值）
    x3 = WARHEAD['Y0']  # 初始高度（真实值）
    x4 = WARHEAD['V0']  # 初始下落速度（真实值）向上为正
    # X = np.array([x1, x2, x3, x4]).reshape(4, 1)  # 此即状态向量的初值 X0
    X = np.array([x1, x2, x3, x4])  # 此即状态向量的初值 X0
    # 定义存储样本的变量
    XX_true = np.zeros((4, len(TT)))  # 下落过程中水平和垂直方向的位置与速度分量（真实值）
    XX_true[:, 0] = X
    RR_true = np.zeros(len(TT))  # 下落过程中雷达测距的真实值
    RR_measured = np.zeros(len(TT))  # 下落过程中雷达测距的测量值
    THETA_true = np.zeros(len(TT))  # 下落过程中雷达测角的真实值
    THETA_measured = np.zeros(len(TT))  # 下落过程中雷达测角的测量值
    [r, theta] = Converto_rNtheta(X[0], X[2], RADAR['R'])
    RR_true[0] = r
    THETA_true[0] = theta

    ### 下落过程仿真
    t = t0
    for k in range(len(TT) - 1):
        X_k = XX_true[:, k]  # 当前时刻状态值
        if (X_k[2] <= RADAR['R']):  # 目标如果接近地面，则停止仿真
            break
        X_next = RK4(X_k, t, h)  # RK4：计算下一时刻状态值
        XX_true[:, k + 1] = X_next  # 存数据
        t = t0 + (k + 1) * h

        x = X_next[0]
        y = X_next[2]
        [r, theta] = Converto_rNtheta(x, y, RADAR['R'])
        RR_true[k + 1] = r  # 记录真实值 r
        THETA_true[k + 1] = theta  # 记录真实值 theta
    # 生成测量值
    RR_measured = RR_true + np.random.normal(0.0, RADAR['sigma_r'], len(RR_true))
    RR_measured[0] = RADAR['r0_measured']
    THETA_measured = THETA_true + np.random.normal(0.0, RADAR['sigma_theta'], len(THETA_true))
    THETA_measured[0] = RADAR['theta0_measured']
    return RR_true, THETA_true, RR_measured, THETA_measured, TT, t  # t 是仿真实际终止时刻


#############################################################
# 程序运行起始点
#############################################################

### 给出 [t0, t0+t] 时间窗口内雷达对目标的距离和角度测量值（RR_measured和THETA_measured）等
[RR_true, THETA_true, RR_measured, THETA_measured, TT, t] = RADAR_MEASURES()

print('实际仿真时长为：', t)
print('设置的时间窗口时长为：', TT[-1])

# 绘图
fig1 = plt.figure()  # 建立一个图的对象
ax = fig1.add_subplot(1, 1, 1)
ax.plot(TT, RR_true, color='blue', linewidth=3, linestyle='-', label='TRUE distance')
ax.plot(TT, RR_measured, color='red', linewidth=1.5, linestyle=':', label='MEASURED distance')
ax.legend(loc='best')
ax.set_title('Distance r of radar measurements')
ax.set_xlabel('t')
ax.set_ylabel('r')
plt.show()

fig2 = plt.figure()  # 建立一个图的对象
ax = fig2.add_subplot(1, 1, 1)
ax.plot(TT, THETA_true, color='blue', linewidth=3, linestyle='-', label='TRUE angle')
ax.legend(loc='best')
ax.set_title('TRUE theta of radar measurements')
ax.set_xlabel('t')
ax.set_ylabel('theta')
plt.show()

fig3 = plt.figure()  # 建立一个图的对象
ax = fig3.add_subplot(1, 1, 1)
ax.plot(TT, THETA_measured, color='red', linewidth=1.5, linestyle=':', label='MEASURED angle')
ax.legend(loc='best')
ax.set_title('MEASURED theta of radar measurements')
ax.set_xlabel('t')
ax.set_ylabel('theta')
plt.show()

### 滤波器程序（学生给出）******


### 结果分析（学生给出）******

class Extend_KalmanFliter:
    def __init__(self):
        self.PHIS = 0.0
        self.Array_T = []
        self.Array_x = []
        self.Array_y = []
        self.Array_yD = []
        self.Array_yDD = []
        self.Array_xH = []
        self.Array_yH = []
        self.Array_yDH = []
        self.Array_yDDH = []
        self.Array_ERR_x = []
        self.Array_ERR_y = []
        self.Array_ERR_yD = []
        self.Array_R = []
        self.Array_TH = []
        self.Array_Beta = []
        self.Array_R_H = []   #_H代表估计值
        self.Array_TH_H = []
        self.Array_Beta_H = []
        self.Array_ERR_R = []
        self.Array_ERR_TH = []
        self.Array_ERR_Beta = []
        self.Array_SP11 = []
        self.Array_SP11_P = []
        self.Array_SP22 = []
        self.Array_SP22_P = []
        self.Array_SP33 = []
        self.Array_SP33_P = []
        self.Array_SP44 = []
        self.Array_SP44_P = []

    def calculate_x_y_yd(self, Ts, X, Y, YD, Beta, H):
        T = 0
        while T <= (Ts - .0001):
            YDD = 0.0034 * 32.2 * YD * YD * np.exp(-Y / 22000.) / (2. * Beta) - 32.2
            YD = YD + H * YDD
            Y = Y + H * YD
            X = X
            T = T + H
        return X, Y, YD

    def KalmanFiltering(self):
        WARHEAD, RADAR, pars = INITIATION()
        ORDER = 4
        PHIS = self.PHIS
        S = 0.
        IDNP = np.eye(ORDER)
        BETA = 500
        BETAH = 800
        BETA_INV = 1 / BETA
        BETAH_INV = 1 / BETAH

        sigma_theta = RADAR['sigma_theta']  # 角度测量精度（rad）
        sigma_r = RADAR['sigma_r']  # 距离测量精度（英尺）

        RMAT = np.array([[sigma_theta ** 2, 0],
                         [0, sigma_r ** 2]])
        P = np.array([
            [1e10, 0, 0, 0],
            [0, (BETA_INV - BETAH_INV) ** 2, 0, 0],
            [0, 0, 1e10, 0],
            [0, 0, 0, 1e10]
        ])

        y_true = WARHEAD['Y0']  # 真实值 y
        x_true = WARHEAD['X0']  # x
        yD_true= WARHEAD['V0']  # yD
        Beta0 = WARHEAD['beta0']
        Beta1 = WARHEAD['beta1']
        T_split = WARHEAD['T_split']
        Ts = RADAR['Ts']
        YR = RADAR['R']

        RH= RADAR['r0_measured']  # 距离初始测量值  RADAR['r0_measured'] = r0 + RADAR['sigma_r']              # 距离初始测量值
        THH = RADAR['theta0_measured']  # 角度初始测量值  THH和RH改名
        [xH, yH] = Converto_xNy(RH, THH)  # xTH  x的初始测量  y的初始测量值   xTH,yTH也改个名
        yDH = yD_true
        G = pars['g']
        T = pars['t0']
        TF = pars['tf']
        H = pars['h']

        while (T <= TF) & (y_true >= 0.):
            if T < T_split:
                Beta = Beta0
                if T == pars['t0']:
                    Beta_Hat = Beta0
            else:
                Beta = Beta1
                if T == T_split:
                    Beta_Hat = Beta1
            #------------------------------------------------------
            # Y = [[x_true], [Beta],[y_true], [yD_true]]
            # Y_next = RK4(Y, T, H)
            # x_true = Y_next[0,0]
            # y_true = Y_next[2,1]
            # yD_true =Y_next[3,1]
            # T = T + H
            xOLD = x_true
            yOLD = y_true
            yDOLD = yD_true
            yDD = .0034 * G * yD_true * yD_true * np.exp(-y_true / 22000.) / (2. * Beta) - G
            x_true = x_true
            y_true = y_true + H * yD_true
            yD_true = yD_true + H * yDD
            #------------------------------------------------------
            T = T + H
            yDD = .0034 * G * yD_true * yD_true * np.exp(-y_true / 22000.) / (2. * Beta) - G
            x_true = .5 * (xOLD + x_true)
            y_true = .5 * (yOLD + y_true + H * yD_true)
            yD_true = .5 * (yDOLD + yD_true + H * yDD)
            #------------------------------------------------------
            S = S + H  # 目标下落过程的数值仿真积分步长（秒）
            if S >= (Ts - .00001):
                S = 0.
                # ------------------------------------------------------
                RHOH = 0.0034 * np.exp(-yH / 22000.)
                f42 = -RHOH * yDH * yDH * G / (2. * Beta_Hat * Beta_Hat)
                f43 = -(RHOH * G * yDH * yDH) / (44000. * Beta_Hat)
                f44 = RHOH * G * yDH / Beta_Hat

                Qk = np.array([
                    [0, 0, 0, 0],
                    [0, Ts * PHIS, 0, f42 * Ts * Ts * PHIS / 2],
                    [0, 0, Ts * Ts * Ts * PHIS / 3, Ts * Ts * PHIS / 2 + f44 * Ts * Ts * Ts * PHIS / 3],
                    [0, f42 * Ts * Ts * PHIS / 2, Ts * Ts * PHIS / 2 + f44 * Ts * Ts * Ts * PHIS / 3,
                     Ts * PHIS + f44 * Ts * Ts * PHIS + (f44 ** 2 + f42 ** 2) * Ts * Ts * Ts * PHIS / 3]
                ])
                F = np.array([[0., 0., 0., 0.],
                              [0., 0., 0., 0.],
                              [0., 0., 0., 1.],
                              [0., f42, f43, f44]])
                PHI = IDNP + F * Ts  # fai_k

                [xB, yB, yDB] = self.calculate_x_y_yd(Ts, xH, yH, yDH, Beta_Hat, H)
                [RB, THB] = Converto_rNtheta(xB, yB, YR)
                HMAT = np.array([[(yB - YR) / (RB ** 2), 0., -xB / (RB ** 2), 0.],
                                 [xB / RB, 0., (yB - YR) / RB, 0.]])

                #Ricatie方程组
                M = np.dot(np.dot(PHI, P), PHI.T) + Qk
                K = np.dot(np.dot(M, HMAT.T), np.linalg.pinv(np.dot(np.dot(HMAT, M), HMAT.T) + RMAT))
                P = np.dot((IDNP - np.dot(K, HMAT)), M)

                THNOISE = sigma_theta * np.random.random()  # 角度测量噪声（rad）
                RNOISE = sigma_r * np.random.random()  # 距离测量噪声（英尺）
                [R, TH] = Converto_rNtheta(x_true, y_true, YR)
                RES1 = TH + THNOISE - THB  # 真实值 + 噪声 = 测量值
                RES2 = R + RNOISE - RB

                xH = xB + K[0, 0] * RES1 + K[0, 1] * RES2
                Beta_Hat = Beta_Hat + K[1, 0] * RES1 + K[1, 1] * RES2
                yH = yB + K[2, 0] * RES1 + K[2, 1] * RES2
                yDH = yDB + K[3, 0] * RES1 + K[3, 1] * RES2

                [RH, THH] = Converto_rNtheta(xH, yH, YR)
                yDDH = 0.0034 * G * yDH * yDH * np.exp(-yH / 22000.) / (2. * Beta_Hat) - G

                ERR_x = x_true - xH
                SP11 = np.sqrt(P[0, 0])
                ERR_Beta = Beta - Beta_Hat
                SP22 = np.sqrt(P[1, 1])
                ERR_y = y_true - yH
                SP33 = np.sqrt(P[2, 2])
                ERR_yD = yD_true - yDH
                SP44 = np.sqrt(P[3, 3])

                ERR_R = R - RH
                ERR_TH = TH - THH
                SP11P = -SP11
                SP22P = -SP22
                SP33P = -SP33
                SP44P = -SP44

                self.Array_T.append(T)
                self.Array_x.append(x_true)
                self.Array_y.append(y_true)
                self.Array_yD.append(yD_true)
                self.Array_yDD.append(yDD)
                self.Array_xH.append(xH)
                self.Array_yH.append(yH)
                self.Array_yDH.append(yDH)
                self.Array_yDDH.append(yDDH)
                self.Array_ERR_x.append(ERR_x)
                self.Array_ERR_y.append(ERR_y)
                self.Array_ERR_yD.append(ERR_yD)
                self.Array_R.append(R)
                self.Array_TH.append(TH)
                self.Array_Beta.append(Beta)
                self.Array_R_H.append(RH)
                self.Array_TH_H.append(THH)
                self.Array_Beta_H.append(Beta_Hat)
                self.Array_ERR_R.append(ERR_R)
                self.Array_ERR_TH.append(ERR_TH)
                self.Array_ERR_Beta.append(ERR_Beta)
                self.Array_SP11.append(SP11)
                self.Array_SP11_P.append(SP11P)
                self.Array_SP22.append(SP22)
                self.Array_SP22_P.append(SP22P)
                self.Array_SP33.append(SP33)
                self.Array_SP33_P.append(SP33P)
                self.Array_SP44.append(SP44)
                self.Array_SP44_P.append(SP44P)
        self.Array_T = np.array(self.Array_T)
        self.Array_x = np.array(self.Array_x)
        self.Array_y = np.array(self.Array_y)
        self.Array_yD = np.array(self.Array_yD)
        self.Array_yDD = np.array(self.Array_yDD)
        self.Array_xH = np.array(self.Array_xH)
        self.Array_yH = np.array(self.Array_yH)
        self.Array_yDH = np.array(self.Array_yDH)
        self.Array_yDDH = np.array(self.Array_yDDH)
        self.Array_ERR_x = np.array(self.Array_ERR_x)
        self.Array_ERR_y = np.array(self.Array_ERR_y)
        self.Array_ERR_yD = np.array(self.Array_ERR_yD)
        self.Array_R = np.array(self.Array_R)
        self.Array_TH = np.array(self.Array_TH)
        self.Array_Beta = np.array(self.Array_Beta)
        self.Array_R_H = np.array(self.Array_R_H)
        self.Array_TH_H = np.array(self.Array_TH_H)
        self.Array_Beta_H = np.array(self.Array_Beta_H)
        self.Array_ERR_R = np.array(self.Array_ERR_R)
        self.Array_ERR_TH = np.array(self.Array_ERR_TH)
        self.Array_ERR_Beta = np.array(self.Array_ERR_Beta)
        self.Array_SP11 = np.array(self.Array_SP11)
        self.Array_SP11_P = np.array(self.Array_SP11_P)
        self.Array_SP22 = np.array(self.Array_SP22)
        self.Array_SP22_P = np.array(self.Array_SP22_P)
        self.Array_SP33 = np.array(self.Array_SP33)
        self.Array_SP33_P = np.array(self.Array_SP33_P)
        self.Array_SP44 = np.array(self.Array_SP44)
        self.Array_SP44_P = np.array(self.Array_SP44_P)


if __name__=='__main__':
    ekf = Extend_KalmanFliter()  # 实例化对象
    ekf.PHIS = 0
    ekf.Array_T = []
    ekf.Array_x = []
    ekf.Array_y = []
    ekf.Array_yD = []
    ekf.Array_yDD = []
    ekf.Array_xH = []
    ekf.Array_yH = []
    ekf.Array_yDH = []
    ekf.Array_yDDH = []
    ekf.Array_ERR_x = []
    ekf.Array_ERR_y = []
    ekf.Array_ERR_yD = []
    ekf.Array_R = []
    ekf.Array_TH = []
    ekf.Array_Beta = []
    ekf.Array_R_H = []  # _H代表估计值
    ekf.Array_TH_H = []
    ekf.Array_Beta_H = []
    ekf.Array_ERR_R = []
    ekf.Array_ERR_TH = []
    ekf.Array_ERR_Beta = []
    ekf.Array_SP11 = []
    ekf.Array_SP11_P = []
    ekf.Array_SP22 = []
    ekf.Array_SP22_P = []
    ekf.Array_SP33 = []
    ekf.Array_SP33_P = []
    ekf.Array_SP44 = []
    ekf.Array_SP44_P = []
    ekf.KalmanFiltering()  # 调用kalman函数

    fig4 = plt.figure()
    ax = fig4.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_x, color='#EE4000', linewidth=1.5, linestyle='-',
            label='True horizontal distance')
    ax.plot(ekf.Array_T, ekf.Array_xH, color='#836FFF', linewidth=1.5, linestyle=':',
            label='Estimated horizontal distance')
    ax.legend(loc='best')
    ax.set_title('The estimate of the horizontal distance x')
    ax.set_xlabel('T/s')
    ax.set_ylabel('x/ft')
    plt.show()

    fig5 = plt.figure()
    ax = fig5.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_y, color='#EE4000', linewidth=1.5, linestyle='-',
            label='TRUE vertical distance')
    ax.plot(ekf.Array_T, ekf.Array_yH, color='#836FFF', linewidth=1.5, linestyle=':',
            label='Estimated vertical distance')
    ax.legend(loc='best')
    ax.set_title('The estimation of the vertical distance y')
    ax.set_xlabel('T/s')
    ax.set_ylabel('y/ft')
    # position = path_dir + "/" + "The estimation of the vertical distance y" + ".png"
    # plt.savefig(position)
    plt.show()

    fig6 = plt.figure()
    ax = fig6.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_yD, color='#EE4000', linewidth=1.5, linestyle='-',
            label='TRUE vertical velocity')
    ax.plot(ekf.Array_T, ekf.Array_yDH, color='#836FFF', linewidth=1.5, linestyle=':',
            label='The estimate of vertical velocity')
    ax.legend(loc='best')
    ax.set_title('The estimation of vertical velocity Vy')
    ax.set_xlabel('T/s')
    ax.set_ylabel('Vy/(ft/s)')
    plt.show()

    fig7 = plt.figure()
    ax = fig7.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_yDD, color='#EE4000', linewidth=1.5, linestyle='-',
            label='TRUE vertical acceleration')
    ax.plot(ekf.Array_T, ekf.Array_yDDH, color='#836FFF', linewidth=1.5, linestyle=':',
            label='The estimated vertical acceleration')
    ax.legend(loc='best')
    ax.set_title('The estimation of vertical acceleration')
    ax.set_xlabel('T/s')
    ax.set_ylabel('Ay/(ft/s**2)')
    plt.grid()
    plt.show()

    fig8 = plt.figure()
    ax = fig8.add_subplot(1, 1, 1)
    ax.plot(TT, RR_true, color='#EE4000', linewidth=1.5, linestyle='-', label='TRUE distance')
    ax.plot(TT, RR_measured, color='#836FFF', linewidth=1.5, linestyle=':', label='MEASURED distance')
    ax.legend(loc='best')
    ax.set_title('The distance between target and radar')
    ax.set_xlabel('T/s')
    ax.set_ylabel('r/ft')
    plt.show()

    fig9 = plt.figure()
    ax = fig9.add_subplot(1, 1, 1)
    ax.plot(TT, THETA_true, color='#EE4000', linewidth=1.5, linestyle='-', label='TRUE theta')
    ax.plot(TT, THETA_measured, color='#836FFF', linewidth=1.5, linestyle=':', label='MEASURED theta')
    ax.legend(loc='best')
    ax.set_title('The theta between target and radar')
    ax.set_xlabel('T/s')
    ax.set_ylabel('theta/rad')
    plt.show()

    fig10 = plt.figure()
    ax = fig10.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_Beta, color='#EE4000', linewidth=1.5, linestyle='-',
            label='TRUE ballistic coefficient')
    ax.plot(ekf.Array_T, ekf.Array_Beta_H, color='#836FFF', linewidth=1.5, linestyle=':',
            label='The estimate of the ballistic coefficient')
    ax.legend(loc='best')
    ax.set_title('The estimation of ballistic coefficient')
    ax.set_xlabel('T/s')
    ax.set_ylabel('Beta')
    plt.xlim((0, 60))
    plt.ylim((0, 1000))
    plt.show()

    fig11 = plt.figure()
    ax = fig11.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_SP11, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical horizontal distance')
    ax.plot(ekf.Array_T, ekf.Array_SP11_P, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical horizontal distance')
    ax.plot(ekf.Array_T, ekf.Array_ERR_x, color='#836FFF', linewidth=1.5, linestyle='-',
            label='Estimated horizontal distance')
    ax.legend(loc='best')
    ax.set_title('Estimation error of horizontal distance x')
    ax.set_xlabel('T/s')
    ax.set_ylabel('x/ft')
    plt.xlim((0, 60))
    plt.ylim((-250, 250))
    plt.show()

    fig12 = plt.figure()
    ax = fig12.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_SP22, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical vertical distance')
    ax.plot(ekf.Array_T, ekf.Array_SP22_P, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical vertical distance')
    ax.plot(ekf.Array_T, ekf.Array_ERR_y, color='#836FFF', linewidth=1.5, linestyle='-',
            label='Estimated vertical distance')
    ax.legend(loc='best')
    ax.set_title('Estimation error of vertical distance y')
    ax.set_xlabel('T/s')
    ax.set_ylabel('y/ft')
    plt.show()

    fig13 = plt.figure()
    ax = fig13.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_SP33, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical vertical velocity')
    ax.plot(ekf.Array_T, ekf.Array_SP33_P, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical vertical velocity')
    ax.plot(ekf.Array_T, ekf.Array_ERR_yD, color='#836FFF', linewidth=1.5, linestyle='-',
            label='Estimated vertical velocity')
    ax.legend(loc='best')
    ax.set_title('Estimation error of vertical velocity Vy')
    ax.set_xlabel('T/s')
    ax.set_ylabel('Vy/(ft/s)')
    plt.xlim((0, 60))
    plt.ylim((-75, 75))
    # position = path_dir + "/" + "Estimation error of vertical velocity Vy" + ".png"
    # plt.savefig(position)
    plt.show()

    fig14 = plt.figure()
    ax = fig14.add_subplot(1, 1, 1)
    ax.plot(ekf.Array_T, ekf.Array_SP44, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical ballistic coefficient')
    ax.plot(ekf.Array_T, ekf.Array_SP44_P, color='#EE4000', linewidth=1.5, linestyle=':',
            label='Theoretical ballistic coefficient')
    ax.plot(ekf.Array_T, ekf.Array_ERR_Beta, color='#836FFF', linewidth=1.5, linestyle='-',
            label='Estimated ballistic coefficient')
    ax.legend(loc='best')
    ax.set_title('Estimation error of ballistic coefficient')
    ax.set_xlabel('T/s')
    ax.set_ylabel('Beta')
    plt.xlim((0, 60))
    plt.ylim((-75, 75))
    plt.show()



